/**
 * @file		statemachine.c
 * @brief		State Machine
 * @note		None
 * @attention	None
 * 
 * <B><I>ALL RIGHTS RESERVED, COPYRIGHT&copy; SOCIONEXT INCORPORATED 2016</I></B>
 */

#include <stdio.h>

/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "event_groups.h"
#include "task.h"
#include "ARMCM0.h"

#include "statemachine.h"
#include "loader.h"
#include "uart.h"
#include "calendar.h"

#include "pmc.h"
#include "dd_pmc_crg.h"
#include "dd_pmc_exiu.h"
#include "dd_ipcu.h"

#include "user_custom.h"
#include "dd_pmc_gpio.h"

#include "dd_pmc.h"
#if defined(BOARD_TYPE_SNI) || defined(BOARD_TYPE_SNI_SMALL)
#include "dd_pmc_i2c.h"
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL

extern void vStartupHW_EXS(void);
extern unsigned custom_PmicGoToStandby( unsigned mode );
extern unsigned custom_PmicWakeupFromStandby( unsigned mode );
extern unsigned custom_DetectBootUpSource( unsigned char* status );
#if defined(BOARD_TYPE_SNI)
UCHAR ucCodeToVoltage[10] = {
	0x05,	// AVS:0x07 - 0.79V -> DD1 Voltage:0x05(=0.80V)
	0x05,	// AVS:0x08 - 0.80V -> DD1 Voltage:0x05
	0x06,	// AVS:0x09 - 0.81V -> DD1 Voltage:0x06(=0.82V)
	0x06,	// AVS:0x0A - 0.82V -> DD1 Voltage:0x06
	0x07,	// AVS:0x0B - 0.83V -> DD1 Voltage:0x07(=0.84V)
	0x07,	// AVS:0x0C - 0.84V -> DD1 Voltage:0x07
	0x08,	// AVS:0x0D - 0.85V -> DD1 Voltage:0x08(=0.86V)
	0x08,	// AVS:0x0E - 0.86V -> DD1 Voltage:0x08
	0x09,	// AVS:0x0F - 0.87V -> DD1 Voltage:0x09(=0.88V)
	0x09,	// AVS:0x10 - 0.88V -> DD1 Voltage:0x09
};
#elif	defined(BOARD_TYPE_SNI_SMALL)
UCHAR ucCodeToVoltage[10] = {
	0x07,	// AVS:0x07 - 0.79V -> DD1 Voltage:0x07(=0.79875V)
	0x07,	// AVS:0x08 - 0.80V -> DD1 Voltage:0x07
	0x08,	// AVS:0x09 - 0.81V -> DD1 Voltage:0x08(=0.81V)
	0x08,	// AVS:0x0A - 0.82V -> DD1 Voltage:0x08
	0x09,	// AVS:0x0B - 0.83V -> DD1 Voltage:0x09(=0.82125V)
	0x0A,	// AVS:0x0C - 0.84V -> DD1 Voltage:0x0A(=0.8325V)
	0x0B,	// AVS:0x0D - 0.85V -> DD1 Voltage:0x0B(=0.84375V)
	0x0C,	// AVS:0x0E - 0.86V -> DD1 Voltage:0x0C(=0.855V)
	0x0D,	// AVS:0x0F - 0.87V -> DD1 Voltage:0x0D(=0.86625V)
	0x0E,	// AVS:0x10 - 0.88V -> DD1 Voltage:0x0E(=0.8775V)
};
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI

#if defined(BOARD_TYPE_SNI) || defined(BOARD_TYPE_SNI_SMALL)
static EventGroupHandle_t xAvsI2cEventFlag;
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL

extern void retarget_init_on_RTOS();

static E_STATEMACHINE_STATE gStatemachineState = STATE_INIT;
static EventGroupHandle_t xStateMachineEventFlag;

unsigned long BootstrapFunction = 0;

unsigned long PmuStatus = 0;

static E_STATE_AFTER_STANDY eStateAfterStandy = STATE_AFTER_STANDY_WAIT;
static unsigned iStateAfterStandby_Time = 0;
static unsigned iStateAfterStandby_Mode = 0;

static unsigned char gstate_boot_source = 0;
#if defined(BOARD_TYPE_SNI) || defined(BOARD_TYPE_SNI_SMALL)
static T_DD_PMC_I2C_ACTION prvAvsI2cSendCallback(E_DD_PMC_I2C_RECV xRecvAction, UCHAR ucRecvData)
{
	T_DD_PMC_I2C_ACTION	xRetAction;
	BaseType_t			xHigherPriorityTaskWoken, xResult;
	
	// Stop.
	Dd_PMC_I2C_Stop_Master();
	
	xHigherPriorityTaskWoken = pdFALSE;
	
	// Set Event Flag : AVS I2C Send.
	xResult = xEventGroupSetBitsFromISR(xAvsI2cEventFlag, 1, &xHigherPriorityTaskWoken);
	if (xResult == pdPASS) {
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
	
	// Set Return value.
	xRetAction.action_mode	= 0;	// no action.
	xRetAction.send_data	= 0;	// no data.
	
	return xRetAction;
}
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL

static int prvSetSdramSelfRefreshMode(unsigned long sdram_addr, int mode) {
	int loop;
	int ret = -1;
	
	if ( mode ) {
		// set self-refresh mode
		// UMCSPCSETA.SRINEN
		*((volatile unsigned long*)(sdram_addr + 0x8080)) = 0x00000100;
		// check self-refresh control state
		for (loop = 0; loop < 1000; loop++) {
			// 1ms wait.
			vTaskDelay( 1 );
			
			// UMCSPCSTAT.SLFRST
			if (*((volatile unsigned long*)(sdram_addr + 0x8090)) == 0x00000001) {
				ret = 0;
				break;
			}
		}
	}
	else {
		// return from self-refresh mode
		// UMCSPCSETA.SROUTEN
		*((volatile unsigned long*)(sdram_addr + 0x8080)) = 0x00000200;
		// check self-refresh control state
		for (loop = 0; loop < 1000; loop++) {
			// 1ms wait.
			vTaskDelay( 1 );
			
			// UMCSPCSTAT.SLFRST
			if (*((volatile unsigned long*)(sdram_addr + 0x8090)) == 0x00000000) {
				ret = 0;
				break;
			}
		}
	}
	
	return ret;
}

INT32 Init_I2C(T_DD_PMC_I2C_CTRL_MASTER *xCtrl, USHORT slave_addr)
{
	xCtrl->bps					= E_DD_PMC_I2C_BPS_400;
	xCtrl->dest_slave_addr_len	= E_DD_PMC_I2C_ADDR_LEN_7;
	xCtrl->dest_slave_addr		= slave_addr;
	return Dd_PMC_I2C_Ctrl_Master(xCtrl);
}
typedef struct {
	UCHAR Data[2];
	UCHAR Data_len;
}
T_I2C_READ_DATA;
void ReadWrite_I2C(T_DD_PMC_I2C_START_INFO *startInfo, T_I2C_READ_DATA *recv)
{
	if(!recv) {
		// Send start
		Dd_PMC_I2C_Start_Master(startInfo, (VP_PMC_I2C_CALLBACK)prvAvsI2cSendCallback);
	}
	else
	{	
		/// Before Read AVS, Write register address first
		//startInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
		// Send Write start
		Dd_PMC_I2C_Start_Master(startInfo, (VP_PMC_I2C_CALLBACK)prvAvsI2cSendCallback);
		// wait
		xEventGroupWaitBits( xAvsI2cEventFlag, 0x1, pdTRUE, pdFALSE, portMAX_DELAY );
				
		// Read parameter
		startInfo->rw_mode		= E_DD_PMC_I2C_RW_MODE_READ;
		startInfo->rw_data_len	= recv->Data_len;
		startInfo->rw_data		= &recv->Data[0];

		// Start Read AVS
		Dd_PMC_I2C_Start_Master(startInfo, (VP_PMC_I2C_CALLBACK)prvAvsI2cSendCallback);
	}
	// wait
	xEventGroupWaitBits( xAvsI2cEventFlag, 0x1, pdTRUE, pdFALSE, portMAX_DELAY );
}

static void prvStateMachineActionInit()
{
	// Get AVS code
	unsigned long ulAvs = Dd_PMC_AVS_Get_AVS() & 0x0000001F;
	
	// check code range
	if ((ulAvs > 0x06) && (ulAvs < 0x11))
	{
#if defined(BOARD_TYPE_SNI)
		// Power IC voltage setting
		int							iRet = 0;
		unsigned char				ucSendData[2];
		T_DD_PMC_I2C_CTRL_MASTER	xCtrl;
		T_DD_PMC_I2C_START_INFO		xStartInfo;
		
		// Event Flag
		xAvsI2cEventFlag = xEventGroupCreate();
		if( NULL == xAvsI2cEventFlag ) {
			// assert
			assertion();
		}
		
		// IC10 IN1 - CM0 Power supply unit.
		// IC10 IN4 - CA7 Power supply unit.
		if(D_DDIM_OK == (iRet = Init_I2C(&xCtrl, 0x2C)) ) {
			xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 2;
			xStartInfo.rw_data		= ucSendData;

			// Send Data
			ucSendData[0] = 0x00;	// register address : 0x00 - DD1 output voltage setting.
			ucSendData[1] = ucCodeToVoltage[ulAvs - 0x07];
			ReadWrite_I2C(&xStartInfo, NULL);
			Dd_PMC_ADVFS_Set_Avs(ucSendData[1]); //Big board had the same V-source
			Dd_PMC_ADVFS_Set_Dvfs(ucSendData[1]); //therefore, no Avs/dvfs
			
			// Send Data
			ucSendData[0] = 0x03;	// register address : 0x03 - DD4 output voltage setting.
			ReadWrite_I2C(&xStartInfo, NULL);
		}
		
		// IC11 IN1 - Core power supply unit for the macro.
		if(D_DDIM_OK == (iRet = Init_I2C(&xCtrl, 0x2D)) ) {
			// Send Data
			ucSendData[0] = 0x00;	// register address : 0x00 - DD1 output voltage setting.
			ReadWrite_I2C(&xStartInfo, NULL);
		}		
		// I2C Stop
		Dd_PMC_I2C_Stop_Master();
		Dd_PMC_I2C_Terminate();
		wait(1539);		// 100MHz : 1539 = 1539 * 130nsec = 200.07usec
#elif defined(BOARD_TYPE_SNI_SMALL)
		// Power IC voltage setting
		int							iRet = 0;
		unsigned char				ucSendData[2];
		T_DD_PMC_I2C_CTRL_MASTER	xCtrl;
		T_DD_PMC_I2C_START_INFO		xStartInfo;
		
		// Event Flag
		xAvsI2cEventFlag = xEventGroupCreate();
		if( NULL == xAvsI2cEventFlag ) {
			// assert
			assertion();
		}
		
		if(D_DDIM_OK == (iRet = Init_I2C(&xCtrl, 0x12)) )
		{//configuration I2C control
			xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 2;
			xStartInfo.rw_data		= ucSendData;
			
			// Send Data, AVS
			ucSendData[0] = 0x10;	// register address : 0x10 - DCDC2_NRMVOLT.
			ucSendData[1] = ucCodeToVoltage[ulAvs - 0x07]; //ulAvs=0xe, 0xe-0x7=7, 7=->C = 0.855V
			//ucSendData[1] = 0x10; // 0.9V
			//ucSendData[1] = 0x4; // 0.765V
			ReadWrite_I2C(&xStartInfo, NULL);
			Dd_PMC_ADVFS_Set_Avs(ucSendData[1]);
			printf("Send AVS[%x]=%x\n", ucSendData[0], ucSendData[1]);
			
			// Send Data, DVFS
			ucSendData[0] = 0x11;	// register address : 0x11 - DCDC3_NRMVOLT.
			ucSendData[1] = 0x10;	// 0.9V
			//ucSendData[1] = 0x19;	// 1.00125V
			ReadWrite_I2C(&xStartInfo, NULL);
			Dd_PMC_ADVFS_Set_Dvfs(ucSendData[1]);
			printf("Send DVFS[%x]=%x\n", ucSendData[0], ucSendData[1]);

		#if 0
			// Send Data
			ucSendData[0] = 0x06;	// register address : 0x06 - DCDC1~DCDC7 bit1~7 on/off.
			ucSendData[1] = 0xFE;	// CH7 on
			ReadWrite_I2C(&xStartInfo, NULL);
		#endif
		}
		// I2C Stop
		Dd_PMC_I2C_Stop_Master();
		Dd_PMC_I2C_Terminate();
		wait(1539);		// 100MHz : 1539 = 1539 * 130nsec = 200.07usec
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL
	}
	// Initialize Calendar
	vCalendarInit();

#if 0
	*((volatile unsigned long*)0x55000100)	|= 0x40000000;
	wait(1000);// wait little bit
	printf("0x55000100 :0x%08x\n", *((volatile unsigned long*)0x55000100));
	printf("0x550000F8 :0x%08x\n", *((volatile unsigned long*)0x550000F8));
	printf("0x550000FC :0x%08x\n", *((volatile unsigned long*)0x550000FC));
	*((volatile unsigned long*)0x55000100)	&= ~0x40000000;
#endif
	
	iStateMachineSetState( STATE_LOAD );
}

static void prvStateMachineActionLoad()
{
	int ercd;
	
#ifdef BOOT_LED
	// Load start : LED11(P25) On.
	Dd_PMC_GPIO_Set_Gpio_Status(E_DD_PMC_GPIO_P25, D_DD_PMC_GPIO_GPIO_STATUS_HIGH);
#endif	// BOOT_LED
	// Load code
	ercd = load_procedure();
	
	if((0 == ercd) && (0 != BootstrapFunction)) {
#if 1
		// Start CA7 with cold reset.
		*((volatile unsigned long*)0x20000000)	= 0xe59ff018;		// reset vector for CA7 on Internal SRAM top
#if 1
		*((volatile unsigned long*)0x20000020)	= (unsigned long)BootstrapFunction;		// start address
#else	// dummy loop for test
		*((volatile unsigned long*)0x20000020)	= 0;		// start address
#endif
#else
		// Warm boot test.
		*((volatile unsigned long*)0x20000000)	= 0xe59f001C;
		*((volatile unsigned long*)0x20000004)	= 0xe5900000;
		*((volatile unsigned long*)0x20000008)	= 0xe3500001;
		*((volatile unsigned long*)0x2000000C)	= 0x0a000002;
		*((volatile unsigned long*)0x20000010)	= 0xe3500002;
		*((volatile unsigned long*)0x20000014)	= 0x0a000001;
		*((volatile unsigned long*)0x20000018)	= 0xeafffffe;
		*((volatile unsigned long*)0x2000001C)	= 0xeafffffe;		// cold boot
		*((volatile unsigned long*)0x20000020)	= 0xeafffffe;		// warm boot
		*((volatile unsigned long*)0x20000024)	= 0x0100fffc;
#endif
		*((volatile unsigned long*)0x2000fffc)	= 1;				// set 'cold' into boot flag for CA7 software
		__DSB();
#ifndef CO_POWER_MEASUREMENT
		*((volatile unsigned long*)0x4d021090)	= 0x00000f3f;		// Reset release of CA7 by CRRRS register
#else
		*((volatile unsigned long*)0x4d021090)	= 0x00000f29;		// Reset release of CA7 by CRRRS register
#endif
	}
	
	vStartupHW_EXS();

	iStateMachineSetState( STATE_NORMAL );
#ifdef BOOT_LED
	// Load end : LED10(P24),LED11(P25) Off.
	Dd_PMC_GPIO_Set_Gpio_Status(E_DD_PMC_GPIO_P24, D_DD_PMC_GPIO_GPIO_STATUS_LOW);
	Dd_PMC_GPIO_Set_Gpio_Status(E_DD_PMC_GPIO_P25, D_DD_PMC_GPIO_GPIO_STATUS_LOW);
#endif	// BOOT_LED
}

static void prvStateMachineActionNormal()
{
	if(FALSE == custom_DetectBootUpSource(&gstate_boot_source))
	{
		if(!gstate_boot_source) {
			printf("Boop up source = %d\n", gstate_boot_source);
		}
	}
}

static void prvStateMachineActionSleep()
{
	int iDelayCount;
	unsigned long ulSrc = 0;
	
#ifdef BOARD_TYPE_SNI_SMALL
	if (eStateAfterStandy == STATE_AFTER_STANDY_POWER_OFF) {
		// Set Term
		iStateMachineSetState( STATE_TERM );
		return;
	}
#endif	// BOARD_TYPE_SNI_SMALL

	// IPCU Mailbox6 Sorce clear wait.
	for (iDelayCount = 0; iDelayCount < 1000; iDelayCount++) {
		Dd_IPCU_Get_MB_Src(6, &ulSrc);
		if (ulSrc == 0) {
			break;
		}
		// 1ms wait.
		vTaskDelay( 1 );
	}
	
	// Get Power status.
	PmuStatus = IO_PMC.PMU.PWR_STR & 0x0000007F;		// PD#0~6
	
	// To make sure that the operation of the PD target macro has been completed.
	
	// Reset Actve of CA7() by CRRRS register
	*((volatile unsigned long*)0x4d021090)	= 0x00000000;		// CPU0 - CPU3
	
	// Clock Off.

	// On chip PG Power off.
	if (PmuStatus != 0) {
		// Set the Power OFF request.
		IO_PMC.PMU.PWR_DN_EN = PmuStatus;
		
		// Power-switch-down done Interrupt Clear.
		IO_PMC.PMU.PMU_INT_CLR.bit.PDNIC = 1;
		
		// Power-switch-down done Interrupt Enalbe.
		IO_PMC.PMU.PMU_INT_EN.bit.PDNIE = 1;
		
		// Power Down SET.
		IO_PMC.PMU.PMU_CTL.word |= 0x0000AA02;
		
		for (iDelayCount = 0; iDelayCount < 100; iDelayCount++) {
			// Check Power-switch-down done Interrupt.
			if (IO_PMC.PMU.PMU_INT_ST.bit.PDNI == 1) {
				break;
			}
			// 1ms wait.
			vTaskDelay( 1 );
		}
		if (iDelayCount == 100) {
			// timeout : 100ms
			printf("Error!  Timeout.\n");
			printf("  Sleep - On chip PG Power off\n");
			
			// Assertion
			assertion();
		}
		
		// Power-switch-down done Interrupt Clear.
		IO_PMC.PMU.PMU_INT_CLR.bit.PDNIC = 1;
		
		// Power OFF request Clear.
		IO_PMC.PMU.PWR_DN_EN = 0;
	}
	
#if (UART_TYPE==UART_TYPE_TERMINAL_CMD)
	// terminal log output wait (1ms).
	vTaskDelay( 1 );
#endif	// UART_TYPE
	
	iStateMachineSetState( STATE_STANDBY );
}

#if 1
extern void vStartupHW(void);

// for statdby mode1 on power IC.
#define PDR_CM0		(0x4DFFB000)
#define DDR_CM0		(0x4DFFB100)
#define EPCR_CM0	(0x4DFFB200)
#define PUDER_CM0	(0x4DFFB300)
#define PUDCR_CM0	(0x4DFFB400)
#endif

static void prvStateMachineActionStandby()
{
#ifdef WARM_BOOT_POWER_CONTROL
	int				iDelayCount;
#endif
	int				iRet = 0;
	
	// Interrupt disable
	NVIC_DisableIRQ(HDMAC0_IRQn);
	NVIC_DisableIRQ(HDMAC1_IRQn);
	NVIC_DisableIRQ(HDMAC2_IRQn);
	NVIC_DisableIRQ(HDMAC3_IRQn);
	NVIC_DisableIRQ(HDMAC4_IRQn);
	NVIC_DisableIRQ(UART_Rx_IRQn);
	NVIC_DisableIRQ(UART_Tx_IRQn);
	NVIC_DisableIRQ(SPI_IRQn);
	NVIC_DisableIRQ(Calendar_IRQn);
	NVIC_DisableIRQ(TIMER32_IRQn);
	NVIC_DisableIRQ(ADC_IRQn);
#ifdef BOARD_TYPE_SNI
	NVIC_DisableIRQ(I2C_IRQn);
#endif	// BOARD_TYPE_SNI
	NVIC_DisableIRQ(WATCHDOG_IRQn);
	NVIC_DisableIRQ(TS_IF_IRQn);
	NVIC_DisableIRQ(CTC_IRQn);
	NVIC_DisableIRQ(IPCU0_IRQn);
	NVIC_DisableIRQ(IPCU1_IRQn);
	NVIC_DisableIRQ(IPCU2_IRQn);
	NVIC_DisableIRQ(IPCU3_IRQn);

	// save DDR-PHY value
	ddr_reg_save_self_refresh();
	
	// set self-refresh mode
	iRet = prvSetSdramSelfRefreshMode(0x58820000, 1);	// ch0
	if (iRet != 0) {
		// Assertion
		assertion();
	}
	iRet = prvSetSdramSelfRefreshMode(0x58840000, 1);	// ch1
	if (iRet != 0) {
		// Assertion
		assertion();
	}
	
	// SYSTICK disable.
	vStateMachineSetSysTick(0);
	
#ifdef WARM_BOOT_POWER_CONTROL
	// Off chip PG Power off.
	// mask the reset sneak from CU
	*((volatile unsigned long*)(0x4DFF8014)) = 0x8000;
	
	// Set PLL bypass mode (100MHz -> 32KHz)
	Dd_PMC_CRG11_Set_CRPLC_PLLBYPASS(1);
	
	// Set the Power OFF request : PD7.
	IO_PMC.PMU.PWR_DN_EN = 0x00000080;
	
	// Set Priority : PD7 = 0(Minimum value).
	IO_PMC.PMU.PWR_DLY_CTL[1].word &= 0x001F1F1F;
	
	// Power-switch-down done Interrupt Clear.
	IO_PMC.PMU.PMU_INT_CLR.bit.PDNIC = 1;
	
	// Power-switch-down done Interrupt Enalbe.
	IO_PMC.PMU.PMU_INT_EN.bit.PDNIE = 1;
	
	// Power Down SET.
	IO_PMC.PMU.PMU_CTL.word |= 0x0000AA02;
	
	for (iDelayCount = 0; iDelayCount < 1000; iDelayCount++) {
		// Check Power-switch-down done Interrupt.
		if (IO_PMC.PMU.PMU_INT_ST.bit.PDNI == 1) {
			break;
		}
		// wait : 1ms
		wait(3);		// 32kHz: 3 = 1.32ms.
	}
	if (iDelayCount == 1000) {
		// timeout : 1s
		printf("Error!  Timeout.\n");
		printf("  Standby - Off chip PG Power off\n");
		
		// Assertion
		assertion();
	}
	
	// Power-switch-down done Interrupt Clear.
	IO_PMC.PMU.PMU_INT_CLR.bit.PDNIC = 1;
	
	// Power OFF request Clear.
	IO_PMC.PMU.PWR_DN_EN = 0;
	
#ifdef BOARD_TYPE_SNI
	// for statdby mode1 on power IC.   GPIO1(P20) L->H
	*((volatile unsigned long*)(PDR_CM0 + 0x04)) = 0x00010000;
	*((volatile unsigned long*)(DDR_CM0 + 0x04)) |= 0x00000001;
	__NOP();
	*((volatile unsigned long*)(PDR_CM0 + 0x04)) = 0x00010001;

	// GPIO External Interrupt enable
	NVIC_EnableIRQ(GPIO_IRQn);
	// Clear Interrupt Request : Ch15
	Dd_PMC_EXIU_Set_EIREQCLR_Ch(15);
	// Mask Release : Ch15
	Dd_PMC_EXIU_Set_EIMASK_Release(15);
#else	// BOARD_TYPE_SNI
	if(custom_PmicGoToStandby(iStateAfterStandby_Mode) == FALSE){
		// Clear Interrupt Request : Ch15
		Dd_PMC_EXIU_Set_EIREQCLR_Ch(15);
		// Mask Release : Ch15
		Dd_PMC_EXIU_Set_EIMASK_Release(15);
	}
#endif	// BOARD_TYPE_SNI
	
#else
	if(custom_PmicGoToStandby(iStateAfterStandby_Mode) == FALSE){
		#if defined(BOARD_TYPE_SNI)
			// GPIO External Interrupt enable
			NVIC_EnableIRQ(GPIO_IRQn);
			// Clear Interrupt Request : Ch15
			Dd_PMC_EXIU_Set_EIREQCLR_Ch(15);
			// Mask Release : Ch15
			Dd_PMC_EXIU_Set_EIMASK_Release(15);
		#elif defined(BOARD_TYPE_SNI_SMALL)
			// Clear Interrupt Request : Ch15
			Dd_PMC_EXIU_Set_EIREQCLR_Ch(15);
			// Mask Release : Ch15
			Dd_PMC_EXIU_Set_EIMASK_Release(15);
		#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL
	}
	// Set PLL bypass mode (100MHz -> 32KHz)
	Dd_PMC_CRG11_Set_CRPLC_PLLBYPASS(1);
#endif
	
	if( 0 != iStateAfterStandby_Time ) {
		wait( (iStateAfterStandby_Time * 25000) / 11 );	// clock=32kHz. 440usec -> 1000000/440
	}
	
#ifdef BOOT_LED
	// Standby mode : LED10(P24) Off.
	Dd_PMC_GPIO_Set_Gpio_Status(E_DD_PMC_GPIO_P24, D_DD_PMC_GPIO_GPIO_STATUS_LOW);
#endif	// BOOT_LED
	
	switch( eStateAfterStandy ) {
		case STATE_AFTER_STANDY_WAIT:
			// wait for interrupt
			__WFI();
			break;
		case STATE_AFTER_STANDY_WAKEUP:
			iStateMachineSetState( STATE_WAKEUP );
			break;
		case STATE_AFTER_STANDY_POWER_OFF:
#if defined(BOARD_TYPE_SNI)
			// to power off for power IC.   GPIO3(P22) L->H
			*((volatile unsigned long*)(PDR_CM0 + 0x04)) = 0x00040000;
			*((volatile unsigned long*)(DDR_CM0 + 0x04)) |= 0x00000004;
			__NOP();
			*((volatile unsigned long*)(PDR_CM0 + 0x04)) = 0x00040004;
#endif	// BOARD_TYPE_SNI
			break;
		default:
			// none
			break;
	}
}

static void prvStateMachineActionWakeup()
{
	int iDelayCount;
	int iRet = 0;
#ifdef BOOT_LED
	// Warm boot start : LED10(P24) On.
	Dd_PMC_GPIO_Set_Gpio_Status(E_DD_PMC_GPIO_P24, D_DD_PMC_GPIO_GPIO_STATUS_HIGH);
#endif	// BOOT_LED
	
#ifdef WARM_BOOT_POWER_CONTROL
#ifdef BOARD_TYPE_SNI
	// return form statdby mode1 on power IC.   GPIO1(P20) H->L
	*((volatile unsigned long*)(DDR_CM0 + 0x04)) |= 0x00000001;
	__NOP();
	*((volatile unsigned long*)(PDR_CM0 + 0x04)) = 0x00010000;
	// wait : 20ms
	wait(46);		// 32kHz: 46 = 46 * 440us = 20.24ms
#endif	// BOARD_TYPE_SNI
	
	// Off chip PG Power on.
	// Set PMU Power ON cycle : PD7 = 0xF0.
	IO_PMC.PMU.PWR_ON_CYC[1].word = 0xF0000000;
	
	// Set the Power ON request : PD7.
	IO_PMC.PMU.PWR_ON_EN0 = 0x00000080;
	
	// Set Priority : PD7 = 0x1F(Maximum value).
	IO_PMC.PMU.PWR_DLY_CTL[1].word |= 0x1F000000;
	
	// Power-switch-ON done Interrupt Clear.
	IO_PMC.PMU.PMU_INT_CLR.bit.PONIC = 1;
	
	// Power-switch-ON done Interrupt Enalbe.
	IO_PMC.PMU.PMU_INT_EN.bit.PONIE = 1;
	
	// Power Up SET.
	IO_PMC.PMU.PMU_CTL.word |= 0x0000AA01;
	
	for (iDelayCount = 0; iDelayCount < 1000; iDelayCount++) {
		// Check Power-switch-ON done Interrupt.
		if (IO_PMC.PMU.PMU_INT_ST.bit.PONI == 1) {
			break;
		}
		// wait : 1ms
		wait(3);		// 32kHz: 3 = 1.32ms.
	}
	if (iDelayCount == 1000) {
		// timeout : 1.32s
		printf("Error!  Timeout.\n");
		printf("  Wake-up - Off chip PG Power on - Power on done Interrupt wait\n");
		
		// Assertion
		assertion();
	}
	
	// Power-switch-ON done Interrupt Clear.
	IO_PMC.PMU.PMU_INT_CLR.bit.PONIC = 1;
	
	// Power ON request clear.
	IO_PMC.PMU.PWR_ON_EN0 = 0x00000000;
	
	// Check that the PLL stabilization wait time has been completed
	for (iDelayCount = 0; iDelayCount < 1000; iDelayCount++) {
		if (IO_PMC.IRSIR.word == 0x00000001) {
			break;
		}
		// wait : 1ms
		wait(3);		// 32kHz: 3 = 1.32ms.
	}
	if (iDelayCount == 1000) {
		// timeout : 1.32s
		printf("Error!  Timeout.\n");
		printf("  Wake-up - Off chip PG Power on - PLL stabilization wait\n");
		
		// Assertion
		assertion();
	}
	
	// Set PLL not bypass mode (32KHz -> 100MHz)
	Dd_PMC_CRG11_Set_CRPLC_PLLBYPASS(0);
	
	// mask release the reset sneak from CU
	*((volatile unsigned long*)(0x4DFF8014)) = 0x0000;
	
	// SYSTICK enable
	vStateMachineSetSysTick(1);
	
	// set Clock and GPIO register
	vStartupHW();
	
	// SDRAMC Start from SelfRefresh
	sdram_self_refresh_start();
#else
	// Set PLL not bypass mode (32KHz -> 100MHz)
	Dd_PMC_CRG11_Set_CRPLC_PLLBYPASS(0);
	
	// set Clock and GPIO register
	vStartupHW();
	
	// SYSTICK enable
	vStateMachineSetSysTick(1);
#endif
	
	printf("Wake-up\n");
	
	// Interrupt enable
	NVIC_EnableIRQ(HDMAC0_IRQn);
	NVIC_EnableIRQ(HDMAC1_IRQn);
	NVIC_EnableIRQ(HDMAC2_IRQn);
	NVIC_EnableIRQ(HDMAC3_IRQn);
	NVIC_EnableIRQ(HDMAC4_IRQn);
	NVIC_EnableIRQ(UART_Rx_IRQn);
	NVIC_EnableIRQ(UART_Tx_IRQn);
	NVIC_EnableIRQ(SPI_IRQn);
	NVIC_EnableIRQ(Calendar_IRQn);
	NVIC_EnableIRQ(TIMER32_IRQn);
	NVIC_EnableIRQ(ADC_IRQn);
#ifdef BOARD_TYPE_SNI
	NVIC_EnableIRQ(I2C_IRQn);
#endif	// BOARD_TYPE_SNI
	NVIC_EnableIRQ(WATCHDOG_IRQn);
	NVIC_EnableIRQ(TS_IF_IRQn);
	NVIC_EnableIRQ(CTC_IRQn);
	NVIC_EnableIRQ(IPCU0_IRQn);
	NVIC_EnableIRQ(IPCU1_IRQn);
	NVIC_EnableIRQ(IPCU2_IRQn);
	NVIC_EnableIRQ(IPCU3_IRQn);

#if defined(BOARD_TYPE_SNI)
	// Mask : Ch15
	Dd_PMC_EXIU_Set_EIMASK_Mask(15);
	// GPIO External Interrupt disable
	NVIC_DisableIRQ(GPIO_IRQn);
#elif defined(BOARD_TYPE_SNI_SMALL)
	if(custom_PmicWakeupFromStandby(iStateAfterStandby_Mode) == FALSE){
		// Mask : Ch15
		Dd_PMC_EXIU_Set_EIMASK_Mask(15);
	}
#endif	// BOARD_TYPE_SNI || BOARD_TYPE_SNI_SMALL
	
	// return from self-refresh mode
	iRet = prvSetSdramSelfRefreshMode(0x58820000, 0);	// ch0
	if (iRet != 0) {
		// Assertion
		assertion();
	}
	iRet = prvSetSdramSelfRefreshMode(0x58840000, 0);	// ch1
	if (iRet != 0) {
		// Assertion
		assertion();
	}
	
	// On chip PG Power on.
	if (PmuStatus != 0) {
		IO_PMC.PMU.PWR_ON_EN0 = PmuStatus;
		
		// Power-switch-ON done Interrupt Clear.
		IO_PMC.PMU.PMU_INT_CLR.bit.PONIC = 1;
		
		// Power-switch-ON done Interrupt Enalbe.
		IO_PMC.PMU.PMU_INT_EN.bit.PONIE = 1;
		
		// Power Up SET.
		IO_PMC.PMU.PMU_CTL.word |= 0x0000AA01;
		
		for (iDelayCount = 0; iDelayCount < 1000; iDelayCount++) {
			// Check Power-switch-ON done Interrupt.
			if (IO_PMC.PMU.PMU_INT_ST.bit.PONI == 1) {
				break;
			}
			// 1ms wait.
			vTaskDelay( 1 );
		}
		if (iDelayCount == 1000) {
			// timeout : 1s
			printf("Error!  Timeout.\n");
			printf("  Wake-up - On chip PG Power on\n");
			
			// Assertion
			assertion();
		}
		
		// Power-switch-ON done Interrupt Clear.
		IO_PMC.PMU.PMU_INT_CLR.bit.PONIC = 1;
		
		// Power ON request clear.
		IO_PMC.PMU.PWR_ON_EN0 = 0x00000000;
	}
	
	// Clock On.
	
	// Start CA7.
	*((volatile unsigned long*)0x20000000)	= 0xe59ff018;		// reset vector for CA7 on Internal SRAM top
	*((volatile unsigned long*)0x20000004)	= 0xeafffffe;
	*((volatile unsigned long*)0x20000008)	= 0xeafffffe;
	*((volatile unsigned long*)0x2000000C)	= 0xeafffffe;
	*((volatile unsigned long*)0x20000010)	= 0xeafffffe;
	*((volatile unsigned long*)0x20000014)	= 0xeafffffe;
	*((volatile unsigned long*)0x20000018)	= 0xeafffffe;
	*((volatile unsigned long*)0x2000001C)	= 0xeafffffe;
	*((volatile unsigned long*)0x20000020)	= (unsigned long)BootstrapFunction;		// start address
	// set 'warm' into boot flag for CA7 software
	*((volatile unsigned long*)0x2000fffc)	= 2;
	__DSB();
	// Reset release of CA7 by CRRRS register.
	*((volatile unsigned long*)0x4d021090)	= 0x00000f3f;
	
	// status clear
	PmuStatus = 0;
	
	vStartupHW_EXS();

	iStateMachineSetState( STATE_NORMAL );
}

#ifdef BOARD_TYPE_SNI_SMALL
static void prvStateMachineActionTerm()
{
	// Software PMIC OFF Control Setting
	int							iRet = 0;
	unsigned char				ucSendData[2];
	T_DD_PMC_I2C_CTRL_MASTER	xCtrl;
	T_DD_PMC_I2C_START_INFO		xStartInfo;
	
	// Event Flag
	xAvsI2cEventFlag = xEventGroupCreate();
	if( NULL == xAvsI2cEventFlag ) {
		// assert
		assertion();
	}

	if(D_DDIM_OK == (iRet = Init_I2C(&xCtrl, 0x12)) ) {
		// Send Data, SOFTOFF
		ucSendData[0] = 0x05;	// register address : 0x05 - SYS_Control
		ucSendData[1] = 0x80;	// Bit7:SOFTOFF On - perform power-off sequence.
		
		xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
		xStartInfo.rw_data_len	= 2;
		xStartInfo.rw_data		= ucSendData;
		// Send start
		Dd_PMC_I2C_Start_Master(&xStartInfo, (VP_PMC_I2C_CALLBACK)prvAvsI2cSendCallback);
		// wait
		xEventGroupWaitBits( xAvsI2cEventFlag, 0x1, pdTRUE, pdFALSE, portMAX_DELAY );
	}
	
	// I2C Stop
	Dd_PMC_I2C_Stop_Master();
	Dd_PMC_I2C_Terminate();
	
	// Wait until the power supply voltage stabilized : 200usec
	wait(1539);		// 100MHz : 1539 = 1539 * 130nsec = 200.07usec
}

#ifdef CTC_COUNT_DOWN_USAGE
#include "dd_pmc_ctc.h"
#endif
#if 1
#include "dd_pmc_tmr32.h"
#include "TemperatureSensor.h"
#include "dd_top.h"
#include "dd_pmc_temp_sensor_if.h"
#endif

#define MAPING_MAX 10
const UCHAR ucHexToVoltage[MAPING_MAX] = {
	0x10, //0.9V
	0x11, //0.911V
	0x12, //0.922V
	0x13, //0.933V
	0x14, //0.945V
	0x15, //0.956V
	0x16, //0.967V
	0x17, //0.978V
	0x18, //0.99V
	0x19 //1.00V
};
const UCHAR ucCpuFrqTbl[MAPING_MAX] = {
	0x2D, //450Mhz
	0x2E, //460Mhz
	0x2F, //470Mhz
	0x30, //480Mhz
	0x31, //490Mhz
	0x34, //520Mhz
	0x37, //550Mhz
	0x3A, //580Mhz
	0x3E, //620Mhz
	0x42 //660Mhz
};
VOID Dd_pmc_timer32_cb( VOID )
{
 	T_DD_PMC_TS_IF_TS_DATA ts_data = {0};
 	T_TEMP_SENSOR_BUFFER temp_conf = {0};
 	static unsigned long temp_buf[5] = {0};
 	static unsigned char timer_counter = 0;

	// Get TS Data.
	if (D_DDIM_OK == Dd_PMC_TS_IF_Get_TS_Data(&ts_data))
	{
		// get data output.
		//Ddim_Print(("--- Get Data ------------------\n"));
		//Ddim_Print(("data_valid         : %d\n", ts_data.ts_data_valid));
		//Ddim_Print(("data               : 0x%08lx\n", ts_data.ts_data));
		if(timer_counter && !(timer_counter % 5)) //5
		{
			unsigned pll_buf[4] = {0};

			temp_buf[4] = (temp_buf[0] + temp_buf[1] + temp_buf[2] + temp_buf[3] + temp_buf[4]) / 5;
			timer_counter = 0;

			Ddim_Print(("temp_buf: 4(Avg)=0x%08lx, 3=0x%08lx, 2=0x%08lx, 1=0x%08lx, 0=0x%08lx\n",
				 temp_buf[4], temp_buf[3], temp_buf[2], temp_buf[1], temp_buf[0]));
			//read temperature buffer
			iTemperatureSensorGetBuffer(&temp_conf);
			GetPLL1FrqBuffer(&pll_buf[0]);
			if(temp_buf[4] < temp_conf.hi_near_temp)
			{
				if(pll_buf[2] == pll_buf[3]) {//cur pll == H_frq pll
					return; //leave
				}
				else {
					pll_buf[2] = pll_buf[3]; //resume to H
				}
			}
			else
			{
				if(temp_buf[4] >= temp_conf.hi_full_temp)
				{
					if(0x0 == pll_buf[2]) {
						return; //leave
					}
					else {
						pll_buf[2] = 0x0; //set cur frq resume to low
					}
				}
				else //if(temp_buf[4] >= temp_conf.hi_near_temp)
				{
					if(pll_buf[2] == pll_buf[3] / 2) {
						return; //leave
					}
					else {
						pll_buf[2] = pll_buf[3] / 2; //set cur Frq to Half high
					}
				}
			}
			SetPLL1FrqBuffer(&pll_buf[0]);
			Dd_PMC_ADVFS_Set_Dvfs(ucHexToVoltage[pll_buf[2]]);
			printf("cur-mapIdx=%u, phy-voltage=0x%x\n", pll_buf[2], ucHexToVoltage[pll_buf[2]]);
			if(iStateMachineSetState( STATE_DVFS_CHANGE ) != 0) {
				printf("case STATE_DVFS_CHANGE err!\n");
			}
		}
		else {
			temp_buf[timer_counter] = ts_data.ts_data; //0, 1, 2, 3, 4
			timer_counter++;
		}
	}
		
}

static void prvStateMachineActionDvfsChg()
{
	unsigned char				en, ucSendData[2];
	T_DD_PMC_I2C_CTRL_MASTER	xCtrl;
	T_DD_PMC_I2C_START_INFO		xStartInfo;
		
	// Event Flag
	xAvsI2cEventFlag = xEventGroupCreate();
	if( NULL == xAvsI2cEventFlag ) {
		// assert
		assertion();
	}

	if(D_DDIM_OK == Init_I2C(&xCtrl, 0x12)) 
	{
		T_DD_PMC_ADVFS_DATA advfs = {0};
		T_I2C_READ_DATA recv = {{0, 0}};

DVFSChg_again:
		xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
		xStartInfo.rw_data_len	= 2;
		xStartInfo.rw_data		= ucSendData;
		
		// Send Data, DVFS
		ucSendData[0] = 0x11;	// register address : 0x11 - DCDC3_NRMVOLT.
		Dd_PMC_ADVFS_Get_Advfs(&advfs);
		ucSendData[1] = (unsigned char)advfs.DVFS;

		ReadWrite_I2C(&xStartInfo, NULL);
		//printf("Send DVFS[%x]=%x\n", ucSendData[0], ucSendData[1]);

		/// Before Read DVFS, Write register address first
		//xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
		xStartInfo.rw_data_len	= 1;
		// Read parameter
		recv.Data_len = 1;
		ReadWrite_I2C(&xStartInfo, &recv);

		if(advfs.DVFS != recv.Data[0]) {
			goto DVFSChg_again;
		}
		printf("DVFS: recv.Data[0]=0x%x\n", recv.Data[0]); //DVFS: recv.Data[0]=0x19
	}
	
	// I2C Stop
	Dd_PMC_I2C_Stop_Master();
	Dd_PMC_I2C_Terminate();
	wait(539);		// 100MHz : 539 * 130nsec = 70.07usec
	
	Dd_PMC_ADVFS_Get_EN(&en);
	if(en) { //enable
		unsigned pll_buf[4] = {0};
		unsigned char ii;

		GetPLL1FrqBuffer(&pll_buf[0]);
		Dd_Top_Set_PLLFREQ1_PLLIDIV(ucCpuFrqTbl[pll_buf[2]]);
		for(ii =1; ii <MAPING_MAX && ucCpuFrqTbl[pll_buf[2]] != Dd_Top_Get_PLLFREQ1_PLLIDIV(); ii++); //confirm CPU-frq = set
		printf("new - CPUFRQ=%d, ii=%u\r\n", Dd_Top_Get_PLLFREQ1_PLLIDIV(), ii);
	}
	///back to normal
	iStateMachineSetState( STATE_NORMAL );
}
static void prvStateMachineActionDvfsFnChg()
{
	unsigned char en =0;
	
	Dd_PMC_ADVFS_Get_EN(&en);
	if(en) //enable
	{
		unsigned char ii;
		unsigned pll_buf[4] = {0};
		T_DD_PMC_I2C_CTRL_MASTER	xCtrl;
		

		// Event Flag
		if(!(xAvsI2cEventFlag = xEventGroupCreate()) ) {
			// assert
			assertion();
		}

		//Check H / L Frq
		GetPLL1FrqBuffer(&pll_buf[0]);
		if(!pll_buf[0] || pll_buf[0] > 0x42) {
			pll_buf[0] = ucCpuFrqTbl[9]; //H-Frq
		}
		if(!pll_buf[1] || pll_buf[1] < 0x2D) {
			pll_buf[1] = ucCpuFrqTbl[0]; //L-Frq
		}

		//search voltage mapping tbl
		for(ii =0; ii < MAPING_MAX && ucCpuFrqTbl[ii] != pll_buf[0]; ii++);
		pll_buf[2] = ii; //set the mapIdx=current Frq
		SetPLL1FrqBuffer(&pll_buf[0]);
		printf("H-Frq=%u, L-Frq=%u, cur_mapIdx=%u\r\n", pll_buf[0], pll_buf[1], pll_buf[2]);
		
		//for(ii =0; ii <MAPING_MAX && !Dd_Top_Get_PLLFREQ1_PLLIDIV(); ii++); //confirm the CPU-FRQ
		//printf("curr - CPUFRQ=%d, ii=%u\r\n", Dd_Top_Get_PLLFREQ1_PLLIDIV(), ii); //curr - CPUFRQ=45, ii=1

		if(D_DDIM_OK == Init_I2C(&xCtrl, 0x12)) 
		{
			unsigned char ucSendData[2] = {0};
			T_DD_PMC_I2C_START_INFO	xStartInfo;
			T_I2C_READ_DATA recv = {{0, 0}};

	EN_DVFS_again:
			xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 2;
			xStartInfo.rw_data		= ucSendData;
			// Send Write DVFS
			ucSendData[0] = 0x11;	// register address : 0x11 - DCDC3_NRMVOLT.			
			ucSendData[1] = ucHexToVoltage[pll_buf[2]]; //DVFS voltage
			ReadWrite_I2C(&xStartInfo, NULL);
			Dd_PMC_ADVFS_Set_Avs(ucSendData[1]);
			//printf("DVFS: 0x%x\n", ucSendData[1]); //DVFS: 0x19

			/// Before Read DVFS, Write register address first
			//xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 1;
			// Read parameter
		#ifdef I2C_CONTINUE_READ
			recv.Data_len = 2, //continue read
		#else
			recv.Data_len = 1;
		#endif
			ReadWrite_I2C(&xStartInfo, &recv);

			if(ucHexToVoltage[pll_buf[2]] != recv.Data[0]) {
				goto EN_DVFS_again;
			}
			printf("DVFS: recv.Data[0]=0x%x\n", recv.Data[0]); //DVFS: recv.Data[0]=0x19
		}

		//set the High Frq
		Dd_Top_Set_PLLFREQ1_PLLIDIV(ucCpuFrqTbl[pll_buf[2]]);
		for(ii =0; ii <MAPING_MAX && ucCpuFrqTbl[pll_buf[2]] != Dd_Top_Get_PLLFREQ1_PLLIDIV(); ii++); //confirm CPU-frq = set
		printf("new - CPUFRQ=%d, ii=%u\r\n", Dd_Top_Get_PLLFREQ1_PLLIDIV(), ii); //new - CPUFRQ=66, ii=1

	#if 1 ///Temperature Sensor
		{
			T_TEMP_SENSOR_BUFFER temp_buff = {0};

			//read temperature buffer
			iTemperatureSensorGetBuffer(&temp_buff);
			if(!temp_buff.hi_full_temp || !temp_buff.lo_full_temp) {
				// if no initial data, then set default to Buffer.
				temp_buff.hi_full_temp	= 0x0000027F;//0x00000140;	//40 degree, 0x280=the timer will shut down unknow
				temp_buff.hi_near_temp	= 0x00000240;//0x00000100;	//32 degree
				temp_buff.lo_near_temp	= 0xFFFFFFD8;	// -5 degree
				temp_buff.lo_full_temp	= 0xFFFFFFB0;	// -10 degree
				iTemperatureSensorSetBuffer(&temp_buff);
			}
			
			// set notification : enable.
			//iTemperatureSensorNotification(1);
			
			// start.
		#ifdef SET_TEMPERATURE_SENSOR_ALL
			if(0!= iTemperatureAllSensorStart()) {
		#else
			if(0!= iTemperatureSensorStart()) {
		#endif
				printf("Temperature Sensor Start err!");
		#ifdef SET_TEMPERATURE_SENSOR_ALL
				iTemperatureAllSensorStop();
		#else
				iTemperatureSensorStop();
		#endif
			}
		}
	#endif

	#ifdef CTC_COUNT_DOWN_USAGE
		{	///Cycle Time Counter
			T_DD_PMC_CTC_Ctrl ctrl = {0};

			// set control data.
			ctrl.interrupt_enable	= 1;
			ctrl.select_wait		= 1;
			ctrl.clock_length		= 0x3FFF;
			// CTC Control.
			if (D_DDIM_OK == Dd_PMC_CTC_Ctrl(&ctrl)) {
				Ddim_Print(("--- CTC Register ---\n"));
				Ddim_Print(("INTCTL : 0x%08lx\n", IO_PMC.CTC.INTCTL.word));
				Ddim_Print(("OBCLEN : 0x%08lx\n", IO_PMC.CTC.OBCLEN.word));
			}
			else {
				goto Stop_CTC;
			}
			if (D_DDIM_OK == Dd_PMC_CTC_Start((VP_CALLBACK)NULL)) {
				Ddim_Print(("CTL     : 0x%08lx\n", IO_PMC.CTC.CTL.word));
				Ddim_Print(("INTSTAT : 0x%08lx\n", IO_PMC.CTC.INTSTAT.word));
				Ddim_Print(("--------------------\n"));
			}
			else {
		Stop_CTC:
				// set stop data.
				ctrl.interrupt_enable	= 0;
				ctrl.select_wait		= 0;
				ctrl.clock_length		= 0;
				// CTC Control.
				Dd_PMC_CTC_Ctrl(&ctrl);
			}
		}
	#endif

	#if 1 ///32bit Reload Timer
		{
	        T_DD_PMC_TMR32_CTRL  tmr32_ctrl;
	        
	        tmr32_ctrl.csr.bit.trg_fixed0 = 0;
	        tmr32_ctrl.csr.bit.uf_fixed1  = 0;
	        tmr32_ctrl.csr.bit.outl       = 0; //output pin level
	        tmr32_ctrl.csr.bit.cnte       = 1; // Timer counter enable
	        tmr32_ctrl.csr.bit.inte       = 1; // Interrupt enable
	        tmr32_ctrl.csr.bit.reld       = 1; // Reload
	        tmr32_ctrl.csr.bit.csl        = 2; // 32div
	        //tmr32_ctrl.csr.bit.csl        = 1; // 8div
	        tmr32_ctrl.csr.bit.rels       = 0; //Reload initial value
	        
	        //tmr32_ctrl.rlr1         = (1687500 - 1); // Timer Count (1sec)
	        tmr32_ctrl.rlr1         = (1687500 *2  - 1); // Timer Count (2sec)
	        tmr32_ctrl.rlr2         = 0;
	        tmr32_ctrl.soft_counter = 1; // Soft counter
	        tmr32_ctrl.pcallback    = Dd_pmc_timer32_cb; // User's Callback Function

	        if (D_DDIM_OK == Dd_PMC_TMR32_Ctrl(&tmr32_ctrl))
	        {
		        if (D_DDIM_OK == Dd_PMC_TMR32_Start()) {
		       	#if 0
			        Ddim_Print( ("-------========= Dd_PMC_TMR32_Ctrl =========--------\n") );
					Ddim_Print( ("csr.trg_fixed0 = %d\n", tmr32_ctrl.csr.bit.trg_fixed0) );
					Ddim_Print( ("csr.uf_fixed1  = %d\n", tmr32_ctrl.csr.bit.uf_fixed1) );
					Ddim_Print( ("csr.outl       = %d\n", tmr32_ctrl.csr.bit.outl) );
					Ddim_Print( ("csr.cnte       = %d\n", tmr32_ctrl.csr.bit.cnte) );
					Ddim_Print( ("csr.inte       = %d\n", tmr32_ctrl.csr.bit.inte) );
					Ddim_Print( ("csr.reld       = %d\n", tmr32_ctrl.csr.bit.reld) );
					Ddim_Print( ("csr.csl        = %d\n", tmr32_ctrl.csr.bit.csl) );
					Ddim_Print( ("csr.rels       = %d\n", tmr32_ctrl.csr.bit.rels) );
					Ddim_Print( ("rlr1           = %d\n", tmr32_ctrl.rlr1) );
					Ddim_Print( ("rlr2           = %d\n", tmr32_ctrl.rlr2) );
					Ddim_Print( ("soft_counter   = %d\n", tmr32_ctrl.soft_counter) );
					Ddim_Print( ("pcallback      = 0x%x\n", tmr32_ctrl.pcallback) );
				#endif
					Ddim_Print( ("Timer32 IO_PMC.TMR.TMCSR.word =[%lx].\n", IO_PMC.TMR.TMCSR.word) );
				}
			}
	    }
	#endif
	}
	else ///disable
	{
		unsigned char ii;
		unsigned pll_buf[4] = {0};
		T_DD_PMC_I2C_CTRL_MASTER	xCtrl;

		// Event Flag
		if(!(xAvsI2cEventFlag = xEventGroupCreate()) ) {
			// assert
			assertion();
		}

		//set the frq to Low
		GetPLL1FrqBuffer(&pll_buf[0]);
		pll_buf[2] = 0; //restore mapping idx
		SetPLL1FrqBuffer(&pll_buf[0]);
		Dd_Top_Set_PLLFREQ1_PLLIDIV(ucCpuFrqTbl[pll_buf[2]]);
		for(ii =0; ii <MAPING_MAX && ucCpuFrqTbl[pll_buf[2]] != Dd_Top_Get_PLLFREQ1_PLLIDIV(); ii++); //confirm CPU-frq = set
		printf("new - CPUFRQ=%d, ii=%u\r\n", Dd_Top_Get_PLLFREQ1_PLLIDIV(), ii); //new - CPUFRQ=45, ii=1

		// set parameter
		if(D_DDIM_OK == Init_I2C(&xCtrl, 0x12)) 
		{
			unsigned char ucSendData[2] = {0};
			T_DD_PMC_I2C_START_INFO	xStartInfo;
			T_I2C_READ_DATA recv = {{0, 0}};

	DIS_DVFS_again:
			xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 2;
			xStartInfo.rw_data		= ucSendData;
			// Send Write DVFS
			ucSendData[0] = 0x11;	// register address : 0x11 - DCDC3_NRMVOLT.
			ucSendData[1] = ucHexToVoltage[0]; //DVFS voltage925
			ReadWrite_I2C(&xStartInfo, NULL);
			Dd_PMC_ADVFS_Set_Avs(ucSendData[1]);
			//printf("DVFS: 0x%x, 0x%x\n", ucSendData[1], ucHexToVoltage[0]); //DVFS: 0x10, 0x10
			
			/// Before Read DVFS, Write register address first
			//xStartInfo.rw_mode		= E_DD_PMC_I2C_RW_MODE_WRITE;
			xStartInfo.rw_data_len	= 1;
			// Read parameter
			recv.Data_len = 1;
			ReadWrite_I2C(&xStartInfo, &recv);
			if(ucHexToVoltage[0] != recv.Data[0]) {
				goto DIS_DVFS_again;
			}
			printf("DVFS: recv.Data[0]=0x%x\n", recv.Data[0]); //DVFS: recv.Data[0]=0x10
		}

		#ifdef SET_TEMPERATURE_SENSOR_ALL
			iTemperatureAllSensorStop();
		#else
			iTemperatureSensorStop();
		#endif
			Dd_PMC_TMR32_Stop();
	}

	if(!gstate_boot_source) {
		gstate_boot_source = 1;
	}

	// I2C Stop
	Dd_PMC_I2C_Stop_Master();
	Dd_PMC_I2C_Terminate();
	wait(539);		// 100MHz : 539 * 130nsec = 70.07usec

	///back to normal
	iStateMachineSetState( STATE_NORMAL );
}
#endif	// BOARD_TYPE_SNI_SMALL

int iStateMachineSetState_sub(E_STATEMACHINE_STATE state, int intr_flag)
{
	int ans = 1;
	BaseType_t	xHigherPriorityTaskWoken, xResult;
	xHigherPriorityTaskWoken = pdFALSE;
	xResult = pdFALSE;

	switch( state ) {
	case STATE_INIT:
		if(STATE_INIT == gStatemachineState) {
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_INIT), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_INIT) );
			}
			ans = 0;
		} else if(STATE_STANDBY == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_INIT), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_INIT) );
			}
			ans = 0;
		}
		break;
	case STATE_LOAD:
		if(STATE_INIT == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_LOAD), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_LOAD) );
			}
			ans = 0;
		}
		break;
	case STATE_NORMAL:
#ifdef BOARD_TYPE_SNI_SMALL	
		if(STATE_LOAD == gStatemachineState ||
			STATE_WAKEUP == gStatemachineState ||
			STATE_DVFS_CHANGE == gStatemachineState ||
			STATE_DVFS_FN_CHANGE == gStatemachineState) {
#else
		if(STATE_LOAD == gStatemachineState || STATE_WAKEUP == gStatemachineState) {
#endif
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_NORMAL), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_NORMAL) );
			}
			ans = 0;
		}
		else if(STATE_WAKEUP == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_NORMAL), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_NORMAL) );
			}
			ans = 0;
		}
		break;
	case STATE_SLEEP:
		if(STATE_NORMAL == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_SLEEP), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_SLEEP) );
			}
			ans = 0;
		}
		break;
	case STATE_STANDBY:
		if(STATE_SLEEP == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_STANDBY), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_STANDBY) );
			}
			ans = 0;
		}
		break;
	case STATE_WAKEUP:
		if(STATE_STANDBY == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_WAKEUP), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_WAKEUP) );
			}
			ans = 0;
		}
		break;
#ifdef BOARD_TYPE_SNI_SMALL
	case STATE_TERM:
		if((STATE_NORMAL == gStatemachineState) || (STATE_SLEEP == gStatemachineState)) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_TERM), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_TERM) );
			}
			ans = 0;
		}
		break;
	case STATE_DVFS_CHANGE:
		if(STATE_NORMAL == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_DVFS_CHANGE), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_DVFS_CHANGE) );
			}
			ans = 0;
		}
	break;
	case STATE_DVFS_FN_CHANGE:
		if(STATE_NORMAL == gStatemachineState) {
			gStatemachineState = state;
			if( intr_flag ) {
				xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_DVFS_FN_CHANGE), &xHigherPriorityTaskWoken );
			} else {
				xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_DVFS_FN_CHANGE) );
			}
			ans = 0;
		}
	break;
#endif	// BOARD_TYPE_SNI_SMALL
	case STATE_TEST1:
		gStatemachineState = state;
		if( intr_flag ) {
			xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_TEST1), &xHigherPriorityTaskWoken );
		} else {
			xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_TEST1) );
		}
		ans = 0;
		break;
	case STATE_TEST2:
		gStatemachineState = state;
		if( intr_flag ) {
			xResult = xEventGroupSetBitsFromISR( xStateMachineEventFlag, (1<<STATE_TEST2), &xHigherPriorityTaskWoken );
		} else {
			xEventGroupSetBits( xStateMachineEventFlag, (1<<STATE_TEST2) );
		}
		ans = 0;
		break;
	case STATE_MAX:
		break;
	default:
		break;
	}
	if( (0 == ans) && (intr_flag) && (xResult == pdPASS)) {
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
	return ans;
}

int iStateMachineSetState(E_STATEMACHINE_STATE state)
{
	return iStateMachineSetState_sub( state, 0 );
}

int iStateMachineSetStateFromISR(E_STATEMACHINE_STATE state)
{
	return iStateMachineSetState_sub( state, 1 );
}

E_STATEMACHINE_STATE iStateMachineGetState()
{
	return gStatemachineState;
}

void vStateMachineSetSysTick(int enable)
{
	// Set SYSTICK
	if (enable) {
		// enable.
		*((unsigned long*)(0xE000E010)) |= 0x00000001;		// bit0=1.
	}
	else {
		// disable.
		*((unsigned long*)(0xE000E010)) &= ~(0x00000001);	// bit0=0.
	}
}

void vStateMachineTask( void *pvParameters )
{
	EventBits_t uxBits;
	
	retarget_init_on_RTOS();
	
	xStateMachineEventFlag = xEventGroupCreate();
	if( NULL == xStateMachineEventFlag ) {
		// error
		for(;;) ;
	}
	
	iStateMachineSetState( STATE_INIT );
	
	for(;;) {
		uxBits = xEventGroupWaitBits(
			xStateMachineEventFlag, /* The event group being tested. */
			0xFFFF,        /* The bits within the event group to wait for. */
			pdTRUE,        /* BIT  should be cleared before returning. */
			pdFALSE,       /* Don't wait for both bits, either bit will do. */
			portMAX_DELAY );/* Wait . */

		if( uxBits & (1<<STATE_INIT) ) {
			printf("State transition : Init\n");
			prvStateMachineActionInit();
		}
		if( uxBits & (1<<STATE_LOAD) ) {
			printf("State transition : Load\n");
			prvStateMachineActionLoad();
		}
		if( uxBits & (1<<STATE_NORMAL) ) {
			printf("State transition : Normal\n");
			prvStateMachineActionNormal();
		}
		if( uxBits & (1<<STATE_SLEEP) ) {
			printf("State transition : Sleep\n");
			prvStateMachineActionSleep();
		}
		if( uxBits & (1<<STATE_STANDBY) ) {
			printf("State transition : Standby\n");
			prvStateMachineActionStandby();
		}
		if( uxBits & (1<<STATE_WAKEUP) ) {
			printf("State transition : Wake-up\n");
			prvStateMachineActionWakeup();
		}
#ifdef BOARD_TYPE_SNI_SMALL
		if( uxBits & (1<<STATE_TERM) ) {
			printf("State transition : Term\r\n");
			prvStateMachineActionTerm();
		}
		if( uxBits & (1<<STATE_DVFS_CHANGE) ) {
			printf("State transition : dvfs change\r\n");
			prvStateMachineActionDvfsChg();
		}
		if( uxBits & (1<<STATE_DVFS_FN_CHANGE) ) {
			printf("State transition : dvfs fn change\r\n");
			prvStateMachineActionDvfsFnChg();
		}
#endif	// BOARD_TYPE_SNI_SMALL
		if( uxBits & (1<<STATE_TEST1) ) {
			printf("State transition : Test 1\r\n");
		}
		if( uxBits & (1<<STATE_TEST2) ) {
			printf("State transition : Test 2\r\n");
		}
	}
}

typedef union {
	unsigned long word;
	struct {
		unsigned long	xxx		:1;		// TODO
		unsigned long			:31;
	} bit;
} U_CMD_INTRP_WAKEUP_COND;

static U_CMD_INTRP_WAKEUP_COND gCmdIntrpWakeupCond = {0};

void vSetWakeupCondition( unsigned long cond )
{
	gCmdIntrpWakeupCond.word = cond;
}

int iSetStateAfterStandby( E_STATE_AFTER_STANDY next_state, unsigned wait_time, unsigned mode )
{
	if( STATE_AFTER_STANDY_MAX <= next_state ) {
		return 1;
	}

	eStateAfterStandy = next_state;
	iStateAfterStandby_Time = wait_time;
	iStateAfterStandby_Mode = mode;
	return 0;
}

void vGetStateBootUpSource( unsigned char* status )
{
	*status = gstate_boot_source;
}

void vSetStandBy_Pmic( unsigned char standby_mode )
{}

void vSetWakeUp_Pmic( unsigned char wakeup_mode )
{}


// aDvfs =============================---------------------------
typedef struct {
	UCHAR H_frq;
	UCHAR L_frq;
	UCHAR Cur_Mapidx;
	UCHAR H_frq_Mapidx;
}
T_FREQUENCY_INFO;
T_FREQUENCY_INFO g_FrqInfo = {0};

int vAdvfs_DvfsSetEN(unsigned char *cmd)
{
	if(2 > *cmd)
	{
		unsigned char en = 0;

		Dd_PMC_ADVFS_Get_EN(&en);
		if(en == *cmd) {
			printf("vAdvfs_DvfsSetEN already set!\n");
			return -2;
		}
	}
	else {
		printf("vAdvfs_DvfsSetEN err!\n");
		return -3;
	}
#if defined(BOARD_TYPE_SNI)
	printf("Big board not implement dvfs currently!\n");
	return -1;
#elif defined(BOARD_TYPE_SNI_SMALL)
	if(STATE_AFTER_STANDY_DVFS_EN == eStateAfterStandy)
	{
		if(iStateMachineSetState( STATE_DVFS_FN_CHANGE ) != 0) {
			printf("case STATE_DVFS_FN_CHANGE err!\n");
			return -1;
		}
	}
#endif
	return Dd_PMC_ADVFS_Set_EN(*cmd);
}
int vAdvfsSetDate(unsigned char *cmd)
{
#if defined(BOARD_TYPE_SNI)
	printf("Big board not implement dvfs currently!\n");
	return -1;
#elif defined(BOARD_TYPE_SNI_SMALL)
	if(0x10 > *cmd || 0x19 < *cmd) {
		printf("err dvfs voltage\n");
		return -1;
	}
	else {
		unsigned char en = 0;

		Dd_PMC_ADVFS_Get_EN(&en);
		if(en) { //check the auto dvfs en?
			printf("Auto dvfs en\n");
			return -2;	
		}
	}

	if(STATE_AFTER_STANDY_ADJUST_DVFS == eStateAfterStandy)
	{
		if(iStateMachineSetState( STATE_DVFS_CHANGE ) != 0) {
			printf("case STATE_DVFS_CHANGE err!\n");
			return -1;
		}
	}
#endif
	return Dd_PMC_ADVFS_Set_Dvfs(*cmd);
}
int SetPLL1FrqBuffer(unsigned *cmd)
{
#if defined(BOARD_TYPE_SNI)
	printf("Big board not implement dvfs currently!\n");
	return -1;
#elif defined(BOARD_TYPE_SNI_SMALL)
	unsigned char ii;
	//search match H-Frq in tbl
	for(ii =0; ii < MAPING_MAX && ucCpuFrqTbl[ii] != cmd[0]; ii++);
	if(ii < MAPING_MAX) { //find it
		g_FrqInfo.H_frq = cmd[0]; //H-Frq
	}
	else { //no match, assign new value
		//printf("cmd[0]-1=%u\n", cmd[0]);
		for(ii =0; ii < MAPING_MAX && cmd[0] > ucCpuFrqTbl[ii]; ii++);
		cmd[0] = g_FrqInfo.H_frq = (unsigned)ucCpuFrqTbl[ii]; //H-Frq
		//printf("cmd[0]-2=%u, ii=%u\n", cmd[0], ii);
	}
	g_FrqInfo.H_frq_Mapidx = ii; //save the H_frq mapping idx
	//set Current-Map_Tbl,
	g_FrqInfo.Cur_Mapidx = cmd[2]; //Current-Map_Tbl

	//force L-Frq @ 900Mhz currently!
	//if(cmd[1] != ucCpuFrqTbl[0]) {
		cmd[1] = g_FrqInfo.L_frq = (unsigned)ucCpuFrqTbl[0]; //L-Frq
	//}
	//else {
	//	g_FrqInfo.L_frq = cmd[1]; //L-Frq	
	//}
	return 0;
#endif
}

void vAdvfs_DvfsGetEN(unsigned *cmd)
{
	unsigned char en;
	
	Dd_PMC_ADVFS_Get_EN(&en);

	// Set retrun value.
	cmd[0] = (unsigned)en;
}
void vAdvfsGetDate(unsigned *cmd)
{
	T_DD_PMC_ADVFS_DATA advfs;
	
	Dd_PMC_ADVFS_Get_Advfs(&advfs);
	
	// Set retrun value.
	cmd[0] = advfs.AVS;
	cmd[1] = advfs.DVFS;
}
void GetPLL1FrqBuffer(unsigned *cmd)
{
	cmd[0] = g_FrqInfo.H_frq; //H-Frq
	cmd[1] = g_FrqInfo.L_frq; //L-Frq
	cmd[2] = g_FrqInfo.Cur_Mapidx; //Current-Map_Tbl
	cmd[3] = g_FrqInfo.H_frq_Mapidx; //H-Frq-Map_Tbl
}
